#! usr/bin/env python

import rospy
from std_msgs.msg import Float64
import math


if __name__ == "__main__":
    rospy.init_node("gazebo_homework2") 

    #这里发布三个节点的数据
    pub1 = rospy.Publisher("//robosim//joint1_position_controller//command",Float64,queue_size=10)
    pub2 = rospy.Publisher("//robosim//joint2_position_controller//command",Float64,queue_size=10)
    pub3 = rospy.Publisher("//robosim//joint3_position_controller//command",Float64,queue_size=10)
            
    #这是对应的三个信息载体        
    msg1 = Float64()
    msg2 = Float64()
    msg3 = Float64()

    while not rospy.is_shutdown():
        alpha = 0.0  #大臂俯仰
        beta = 0.0  #小臂俯仰
        gama = 0.0  #基座的偏航
        xraw = input("x:")
        if(xraw == "q"):
            break
        yraw = input("y:")
        if(yraw == "q"):
            break
        zraw = input("z:")
        if(zraw == "q"):
            break
        x = float(xraw)
        y = float(yraw)
        z = float(zraw)

        a1 = 0.3 #手臂的长度
        a2 = 0.3
        z0 = 0.25 #关节初始位置z

        dis = (z-z0)**2+x**2+y**2 #距离
        if (dis > 0.5 or z<0.35):
            print("位置不可达")
            continue
        gama = math.atan(y/x)
        if(x < 0.0 and y > 0.0):
            gama+=3.14
        elif(x<0.0 and y < 0.0):
            gama+=3.14
        horizonX = (x**2+y**2)**0.5
        ralZ = z-z0
        q0 = math.atan(ralZ/horizonX)
        a0 = (horizonX**2+(z-z0)**2)**0.5
        temp1 = float((horizonX**2+(z-z0)**2+a1**2-a2**2)/(2*a0*a1))
        if temp1 > 1.0:
            print("位置不可达")
            continue
        alpha = math.acos(temp1)+q0

        temp1 = float((horizonX**2+(z-z0)**2-a1**2+a2**2)/(2*a0*a2))
        if temp1 > 1.0:
            print("位置不可达")
            continue
        beta = -math.acos(temp1)+q0-alpha


        msg1.data = gama  
        msg2.data = 1.57-alpha
        msg3.data = -beta
        pub1.publish(msg1)  #发布数据
        pub2.publish(msg2)
        pub3.publish(msg3)
        rospy.loginfo("正在输出数据，其中alpha:%.2f  beta:%.2f  gama:%.2f",alpha,beta,gama)
        #调用休眠函数
        #rate.sleep()





